package com.testcolision.deprecated;

import java.io.InputStream;
import java.util.ArrayList;

import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.graphics.Mesh;
import com.badlogic.gdx.graphics.g3d.loaders.obj.ObjLoader;
import com.badlogic.gdx.math.Matrix4;
import com.badlogic.gdx.math.Vector3;
import com.collision.CollisionMesh;
import com.collision.Constraint;
import com.collision.EllipsoidCollider;
import com.collision.SlideResponse;
import com.testcolision.simulation.Camera3D;

public class Simulation {
	public final static float PLAYFIELD_MIN_X = -14;
	public final static float PLAYFIELD_MAX_X = 14;
	public final static float PLAYFIELD_MIN_Z = -50;
	public final static float PLAYFIELD_MAX_Z = 50;

	public final static int FRONT_RIGHT_WHEEL = 0;
	public final static int FRONT_LEFT_WHEEL = 1;
	public final static int REAR_RIGHT_WHEEL = 2;
	public final static int REAR_LEFT_WHEEL = 3;

	public final static float CONSTRAINT_DISTANCE = 1f;
	public ConstraintSphere[] constraintSpheres;
	public CollisionMesh cMesh;
	public EllipsoidCollider[] colliderArray;
	public Constraint[] constraints;

	public Mesh m;
	public InputStream in;
	public Car car;
	public Camera3D camera;
	public Matrix4 steerMatrix;

	public Simulation() {
		camera = new Camera3D(new Vector3());
		car = new Car();

		constraintSpheres = new ConstraintSphere[4];
		colliderArray = new EllipsoidCollider[4];
		colliderArray = new EllipsoidCollider[4];
		constraints = new Constraint[6];

		steerMatrix = new Matrix4();

		Vector3 spherePos = new Vector3();

		for (int i = 0; i < 4; i++) {

			switch (i) {
			case FRONT_RIGHT_WHEEL:
				spherePos.set(0, 0f, -500);
				break;
			case FRONT_LEFT_WHEEL:
				spherePos.set(-1, 0f, -500);
				break;
			case REAR_RIGHT_WHEEL:
				spherePos.set(-1, 0f, -501);
				break;
			case REAR_LEFT_WHEEL:
				spherePos.set(0, 0f, -501);
				break;
			}
			constraintSpheres[i] = new ConstraintSphere(spherePos);
			colliderArray[i] = new EllipsoidCollider(0.5f, 0.5f, 0.5f, new SlideResponse());
		}

		constraints[0] = new Constraint(constraintSpheres[0], constraintSpheres[1], CONSTRAINT_DISTANCE);
		constraints[1] = new Constraint(constraintSpheres[1], constraintSpheres[2], CONSTRAINT_DISTANCE);
		constraints[2] = new Constraint(constraintSpheres[2], constraintSpheres[3], CONSTRAINT_DISTANCE);
		constraints[3] = new Constraint(constraintSpheres[3], constraintSpheres[0], CONSTRAINT_DISTANCE);
		constraints[4] = new Constraint(constraintSpheres[1], constraintSpheres[3], CONSTRAINT_DISTANCE);
		constraints[5] = new Constraint(constraintSpheres[2], constraintSpheres[0], CONSTRAINT_DISTANCE);

		try {
			in = Gdx.files.internal("data/race_track.obj").read();
			m = ObjLoader.loadObj(in);
			in.close();
		} catch (Exception e) {

		}
		cMesh = new CollisionMesh(m, false);

	}

	public void update(float delta) {

		if (delta > 1)
			delta = 0.01f;
		car.pos.set(constraintSpheres[0].pos.cpy().mul(0.25f)).add(constraintSpheres[1].pos.cpy().mul(0.25f)).add(constraintSpheres[2].pos.cpy().mul(0.25f))
				.add(constraintSpheres[3].pos.cpy().mul(0.25f));
		car.directionVec.set(constraintSpheres[0].pos).sub(constraintSpheres[3].pos).nor();
		car.sideVec.set(constraintSpheres[2].pos).sub(constraintSpheres[3].pos).nor();
		car.upVec.set(car.sideVec).crs(car.directionVec).nor();
		car.orientationMatrix.setToLookAt(car.directionVec, car.upVec);
		car.orientationMatrix.tra();
		car.angle = constraintSpheres[0].angle;
		// wheels

//		wheels[2].pos.set(constraintSpheres[2].pos);
//		wheels[2].orientationMatrix.setToLookAt(car.directionVec, car.upVec);
//		wheels[2].orientationMatrix.tra();
//
//		wheels[3].pos.set(constraintSpheres[3].pos);
//		wheels[3].orientationMatrix.setToLookAt(car.directionVec, car.upVec);
//		wheels[3].orientationMatrix.tra();
//
//		wheels[0].pos.set(constraintSpheres[0].pos);
//		wheels[0].orientationMatrix.setToLookAt(car.directionVec, car.upVec);
//		wheels[0].orientationMatrix.tra();
//
//		wheels[1].pos.set(constraintSpheres[1].pos);
//		wheels[1].orientationMatrix.setToLookAt(car.directionVec, car.upVec);
//		wheels[1].orientationMatrix.tra();

//		for (int i = 0; i < wheels.length; i++) {
//			wheels[i].update(car.directionVec, car.sideVec, constraintSpheres[3].speed);
//		}

		for (int i = 0; i < constraintSpheres.length; i++) {
			// if (!colliderArray[i].collide(cMesh, carWheel[i].pos, new
			// Vector3(carWheel[i].vel.x * delta, carWheel[i].gravity ,
			// carWheel[i].vel.z * delta), 0.0000001f)) {
			// constraintSpheres[i].gravity -= 0.3;
			colliderArray[i].collide(cMesh, constraintSpheres[i].pos, new Vector3(constraintSpheres[i].vel.x * delta, constraintSpheres[i].gravity * delta,
					constraintSpheres[i].vel.z * delta), 0.0000001f);
			// }

			constraintSpheres[i].update(delta, car.directionVec, car.upVec);
			// constraintSpheres[i].gravity *= 0.9f;

			// carWheel[i].pos.add(carWheel[i].vel.x * delta, carWheel[i].vel.y
			// * delta, carWheel[i].vel.z * delta);

		}

		for (int i = 0; i < constraints.length; i++) {
			constraints[i].solve(delta);
		}

		for (int i = 0; i < constraintSpheres.length; i++) {

			constraintSpheres[i].pos.add(constraintSpheres[i].vel.x * delta, constraintSpheres[i].vel.y * delta, constraintSpheres[i].vel.z * delta);

		}

		// for (int i = 0; i < 2; i++) {
		//
		// wheels[i].orientationMatrix.setToRotation(car.upVec,
		// constraintSpheres[i].angle);
		// }

		// camera.interpolate(car.pos, car.upVec, car.directionVec,
		// constraintSpheres[3].vel, delta);
		camera.update(car.pos, car.upVec, car.directionVec, car.sideVec, delta);
	}

	public void accelerate(float delta, int dir) {

		// for (int i = 0; i < constraintSpheres.length; i++)
		// constraintSpheres[i].accelerate(delta, dir);

	}

	public void steerLeft(float delta, float scale) {
		// for (int i = 0; i < 2; i++) {
		// constraintSpheres[i].steerLeft(delta);
		// }

	}

	public void steerRight(float delta, float scale) {
		// for (int i = 0; i < 2; i++) {
		// constraintSpheres[i].steerRight(delta);
		// }
	}

	float lastX = -1;
	float lastY = -1;

	public void rotateCamera(int x, int y) {

		if (lastX == -1) {
			lastX = x;
			lastY = y;
		} else {
			camera.rotate((x - lastX) / 10, (y - lastY) / 10);
			lastX = x;
			lastY = y;
		}

	}
}
